function [ out, auxOut ] = gentrajectory( in, cfg, par, iniCon, cst )
%GENTRAJECTORY 生成轨迹并进行导航仿真
%
% Tips:
% 1. 地球坐标系选择EUGw，导航坐标系使用ENU地理坐标系g
% 2. 输出的位置、速度、距离包含各杆臂点的输出，次序为V系原点、位移计杆臂点、惯组杆臂点
%
% Input Arguments:
% in.genFuncName: 字符串，轨迹输入量生成函数名称
% in.genFuncInArg: 元胞向量，轨迹输入量生成函数参数
% in.vVRate, in.omegaNVV, in.omegaVBB, in.tSects: 轨迹输入量，当in.genFuncName不存在或为空时使用这三个域
% in.nav_fB, in.nav_omegaIBB: 导航子系统的输入量，当cfg.enableTrajGen为false时使用这两个域，默认（不存在此域时，下同）为0
% cfg.enableCBVdynamic: 逻辑标量，true表示使能惯组旋转调制，默认为false
% cfg.enableTrajGen: 逻辑标量，true表示使能轨迹生成子系统，默认为true
% cfg.enableIMU: 逻辑标量，true表示使能惯组子系统，默认为true
% cfg.enableDisplm: 逻辑标量，true表示使能位移计子系统，默认为false
% cfg.enableNavigation: 逻辑标量，true表示使能导航子系统（用于验证轨迹生成子系统的输出），默认为false
% cfg.forceRerun: 逻辑标量，true表示强制重新运行仿真（即使输入参数与上次相同且没有使能噪声），默认为false
% cfg.IMU.enableErr: 逻辑标量，true表示使能惯组误差生成子系统，默认为true
% cfg.IMU.enableNoise: 逻辑标量，true表示使能惯组噪声生成子系统，默认为false
% cfg.IMU.ignore2OdErr: 逻辑标量，true表示忽略惯组误差模型中的二阶小量，默认为true
% cfg.IMU.quantizeOut: 逻辑标量，true表示对惯组输出进行量化，使各周期的输出量为整数，默认为false
% cfg.IMU.outFilePath: 字符串，指定惯组输出文件的路径，若为空则不生成惯组输出文件，默认为空
% cfg.IMU.outPrecision: 字符串，惯组输出的精度，默认为'%.16e'
% cfg.displm.enableErr: 逻辑标量，true表示使能位移计误差生成子系统，默认为true
% cfg.displm.enableNoise: 逻辑标量，true表示使能位移计噪声生成子系统，默认为false
% cfg.displm.quantizeOut: 逻辑标量，true表示对位移计输出进行量化，使各周期的输出量为整数，默认为false
% par.samplePeriod: 标量，惯组输出的采样周期，单位s
% par.g: 标量，重力加速度，单位m/s^2
% par.IMU.acc.PS0B: 3*3矩阵, 传感器系与本体系之间的坐标转换矩阵标称值，无单位，默认全为单位阵
% par.IMU.acc.KScal0： 3*j（j为1或者轨迹段数，当为1时各段参数相同，下同）向量，各轨迹段加速度计标度因数，单位(^/s)/(m/s^2)，默认为1
% par.IMU.acc.dfBiasS: 3*j向量，各轨迹段加速度计零偏，单位m/s^2，默认为0
% par.IMU.acc.dKScalP: 3*x*j向量，各轨迹段加速度计正向标度因数误差，x为考虑的标度因数误差的阶次，x=1，模型为线性，单位与阶次相关，第1列无单位，第i列单位(m/s^2)^-(i-1)，默认全为0
% par.IMU.acc.dKScalN: 3*x*j向量，各轨迹段加速度计负向标度因数误差，x为考虑的标度因数误差的阶次，x=1，模型为线性，单位与阶次相关，第1列无单位，第i列单位(m/s^2)^-(i-1)，默认全为0
% par.IMU.acc.dPSS0: 3*3*j矩阵，各轨迹段加速度计失准角，单位rad，默认为0
% par.IMU.acc.dfQuantS: 3*j向量，各轨迹段加速度计量化误差，单位1/(m/s^2)，默认为0
% par.IMU.acc.lB: 3*3*j向量，各轨迹段加速度计杆臂项，按列分别为3只加速度计的杆臂项，单位：m，默认全为0
% par.IMU.acc.KAniso: 3*j向量，各轨迹段加速度计不等惯量误差的系数，单位：(m/s^2)/(rad/s)^2，默认全为0
% par.IMU.acc.CBA: 3*3*3*j向量，各轨迹段本体系到3只加速度计坐标系的坐标转换矩阵，无单位，默认各页全为单位阵
% par.IMU.acc.noise.R: 3*1向量，加速度计比力斜坡系数，单位m/s^3，默认为0
% par.IMU.acc.noise.K: 3*1向量，加速度计比力随机游走系数，单位m/s^(5/2)，默认为0
% par.IMU.acc.noise.N: 3*1向量，加速度计比速度随机游走系数，单位m/s^(3/2)，默认为0
% par.IMU.acc.noise.Q: 3*1向量，加速度计量化噪声系数，单位m/s，默认为realmin
% par.IMU.acc.noise.qc: 3*1向量，加速度计指数相关噪声驱动白噪声的功率谱密度平方根，单位m/s^(5/2)，默认为0
% par.IMU.acc.noise.Tc: 3*1向量，加速度计指数相关噪声相关时间，单位s，默认为Inf
% par.IMU.acc.noise.Omega0: 3*1向量，加速度计正弦噪声幅值，单位m/s^2，默认为0
% par.IMU.acc.noise.f0: 3*1向量，加速度计正弦噪声频率，单位Hz，默认为1
% par.IMU.gyro.PS0B: 3*3矩阵, 传感器系与本体系之间的坐标转换矩阵标称值，无单位，默认全为单位阵
% par.IMU.gyro.KScal0: 3*j（j为1或者轨迹段数，当为1时各段参数相同，下同）向量，各轨迹段陀螺仪标度因数，单位(^/s)/(rad/s)，默认为1
% par.IMU.gyro.domegaIBBiasS: 3*j向量，各轨迹段陀螺零偏，单位rad/s，默认为0
% par.IMU.gyro.dKScalP: 3*x*j向量，各轨迹段陀螺正向标度因数误差，x为考虑的标度因数误差的阶次，x=1，模型为线性，单位与阶次相关，第1列无单位，第i列单位(rad/s)^-(i-1)，默认全为0
% par.IMU.gyro.dKScalN: 3*x*j向量，各轨迹段陀螺负向标度因数误差，x为考虑的标度因数误差的阶次，x=1，模型为线性，单位与阶次相关，第1列无单位，第i列单位(rad/s)^-(i-1)，默认全为0
% par.IMU.gyro.dPSS0: 3*3*j矩阵，各轨迹段陀螺失准角，单位rad，默认为0
% par.IMU.gyro.domegaIBQuantS: 3*j矩阵，各轨迹段陀螺量化误差，单位：rad/s，默认全为0
% par.IMU.gyro.DGSens: 3*3*j矩阵，各轨迹段陀螺g敏感项，单位rad/s/(m/s^2)，默认为0
% par.IMU.gyro.noise.R: 3*1向量，陀螺角速度斜坡系数，单位rad/s^2，默认为0
% par.IMU.gyro.noise.K: 3*1向量，陀螺角速度随机游走系数，单位rad/s^(3/2)，默认为0
% par.IMU.gyro.noise.N: 3*1向量，陀螺角度随机游走系数，单位rad/sqrt(s)，默认为0
% par.IMU.gyro.noise.Q: 3*1向量，陀螺量化噪声系数，单位rad，默认为realmin
% par.IMU.gyro.noise.qc: 3*1向量，陀螺指数相关噪声驱动白噪声的功率谱密度平方根，单位rad/s^(3/2)，默认为0
% par.IMU.gyro.noise.Tc: 3*1向量，陀螺指数相关噪声相关时间，单位s，默认为Inf
% par.IMU.gyro.noise.Omega0: 3*1向量，陀螺正弦噪声幅值，单位rad/s，默认为0
% par.IMU.gyro.noise.f0: 3*1向量，陀螺正弦噪声频率，单位Hz，默认为1
% par.IMU.lV: 3*1向量, 惯组相对于V系的杆臂参数，单位m，默认为0
% par.displm.SF0: 3*1向量，位移计标度因数，单位(^/s)/m，默认为1
% par.displm.dSF: 3*1向量，位移计标度因数误差，无单位，默认为0
% par.displm.noise.N: 3*1向量，位移计位移随机游走系数，单位m/sqrt(s)，默认为0
% par.displm.lV: 标量，位移计相对于V系杆臂，默认为0
% iniCon.CVG: 3*3矩阵，G系相对于V系的初始姿态矩阵
% iniCon.CBV：3*3矩阵，V系相对于惯组坐标系的初始姿态矩阵，默认为单位矩阵
% iniCon.vG: 3*1向量，初始速度，单位m/s
% iniCon.pos：3*1向量，分别为初始纬度、经度、高度，单位分别为rad、rad、m
% cst.RE: 标量，地球长半轴，单位m
% cst.omegaIE: 标量，地球自转角速率，单位rad/s
% cst.eSq: 标量，地球偏心率的平方，无单位
% cst.f: 标量，地球扁率，无单位
%
% Output Arguments:
% out.t: n*1向量，轨迹时间，单位s
% out.CVG: 3*3*n矩阵，n为采样数，V系轨迹姿态矩阵
% out.CVL: 3*3*n矩阵，V系轨迹姿态矩阵（相对于L系）
% out.vehicleAtt: n*3矩阵，V系轨迹姿态，分别为绕G系Z、Y、X顺序旋转得到V系的欧拉角，单位rad
% out.CBG: 3*3*n矩阵，n为采样数，B系轨迹姿态矩阵
% out.CBL: 3*3*n矩阵，B系轨迹姿态矩阵（相对于L系）
% out.bodyAtt: n*3矩阵，B系轨迹姿态，分别为绕G系Z、Y、X顺序旋转得到B系的欧拉角，单位rad
% out.vG: n*3*m矩阵，m为杆臂个数，杆臂端点速度，单位m/s
% out.pos: n*3*m矩阵，杆臂端点位置，分别为纬度、经度、高度，单位分别为rad、rad、m
% out.RE: n*3*m矩阵，E系下的杆臂端点位置坐标
% out.s: n*m向量，杆臂端点里程，单位m
% out.omegaNVV: n*3矩阵，V系下的V系相对于导航系的角速度，单位rad/s
% out.omegaNBB: n*3矩阵，B系下的B系相对于导航系的角速度，单位rad/s
% out.IMU.fB: n*3矩阵，加入惯组误差后的比力值，单位m/s^2
% out.IMU.omegaIBB: n*3矩阵，加入惯组误差后的角速度值，单位rad/s
% out.IMU.fBErrFree: n*3矩阵，轨迹比力值，单位m/s^2
% out.IMU.omegaIBBErrFree: n*3矩阵，轨迹角速度值，单位rad/s
% out.IMU.specVelInc: (n-1)*3矩阵，加入惯组误差后各采样周期内的比速度增量值，单位m/s
% out.IMU.angleInc: (n-1)*3矩阵，加入惯组误差后各采样周期内的角增量值，单位rad
% out.IMU.specVelIncErrFree: (n-1)*3矩阵，各采样周期内的比速度增量无误差值，单位m/s
% out.IMU.angleIncErrFree: (n-1)*3矩阵，各采样周期内的角增量无误差值，单位rad
% out.IMU.accOut: (n-1)*3矩阵，加速度计在各采样周期内的输出，单位^
% out.IMU.gyroOut: (n-1)*3矩阵，陀螺在各采样周期内的输出，单位^
% out.IMU.accOutErrFree: (n-1)*3矩阵，加速度计在各采样周期内的无误差输出，单位^
% out.IMU.gyroOutErrFree: (n-1)*3矩阵，陀螺在各采样周期内的无误差输出，单位^
% out.displm.vV: n*3矩阵，加入误差后的V系速度值，单位m/s
% out.displm.vVErrFree: n*3矩阵，轨迹V系速度值，单位m/s
% out.displm.displInc: (n-1)*3矩阵，加入误差后各采样周期内的位移增量值，单位m
% out.displm.displIncErrFree: (n-1)*3矩阵，各采样周期内的位移增量无误差值，单位m
% out.displm.displmOut: (n-1)*3矩阵，加入误差后位移计在各采样周期内的位移增量输出，单位^
% out.displm.displmOutErrFree: (n-1)*3矩阵，位移计在各采样周期内的无误差位移增量输出，单位^
% auxOut.nav.CBG: 3*3*n矩阵，导航子系统输出的姿态矩阵
% auxOut.nav.att: n*3矩阵，导航子系统输出的姿态
% auxOut.nav.vG: n*3矩阵，导航子系统输出的速度
% auxOut.nav.pos: n*3矩阵，导航子系统输出的位置
% auxOut.TGI.vVRate, auxOut.TGI.omegaNVV: 轨迹输入量
% auxOut.TGI.tSects: 各轨迹段的分隔时间点，单位s
%
% References:
% [1] 理论文档“轨迹发生器”

persistent p_genTraj_in p_genTraj_cfg p_genTraj_par p_genTraj_iniCon p_genTraj_cst p_genTraj_out p_genTraj_auxOut

%% 载入默认配置及参量
load('gentrajdefcfgpar.mat');
cfg = structcpy_specfields(defCfg, cfg);
par = structcpy_specfields(defPar, par);

%% 判断是否需要重新仿真
% 如果输入参数与上次相同且没有包含噪声，则不重复进行仿真，直接输出上次保留的输出参数
if ~cfg.forceRerun && isequal(p_genTraj_in, in) && isequal(p_genTraj_cfg, cfg) && isequal(p_genTraj_par, par) ...
        && isequal(p_genTraj_iniCon, iniCon) && isequal(p_genTraj_cst, cst) && ~cfg.IMU.enableNoise && ~cfg.displm.enableNoise
    out = p_genTraj_out;
    auxOut = p_genTraj_auxOut;
    disp('输入参数与上次调用相同且未使能噪声，直接输出上次轨迹');
    return;
end
originalIn = in;
originalCfg = cfg;
originalPar = par;
originalIniCon = iniCon;
originalCst = cst;

%% 配置
cfg.simOptions = simset('DstWorkspace', 'current', 'SrcWorkspace', 'current');

%% 常量
cst.deg2Rad = pi / 180;
cst.CNL = [0 1 0; 1 0 0; 0 0 -1];

%% 输入量
if isfield(in, 'genFuncName') && ~isempty(in.genFuncName)
    tmp = [in.genFuncName '('];
    if isfield(in, 'genFuncInArg')
        for i=1:length(in.genFuncInArg)
            if i > 1
                tmp = [tmp ', ']; %#ok<AGROW>
            end
            tmp = [tmp 'in.genFuncInArg{' int2str(i) '}']; %#ok<AGROW>
        end
    end
    tmp = [tmp ')'];
    [in.vVRate, in.omegaNVV, in.omegaNVVRate, par.tSects] = eval(tmp); % NOTE: tSects中应包含轨迹起始时刻及结束时刻
    disp('完成轨迹输入生成');
else
    par.tSects = in.tSects;
end
if ~isfield(in, 'omegaVBB')
    in.omegaVBB.time = 0;
    in.omegaVBB.signals.values = zeros(1, 3);
    in.omegaVBB.signals.dimensions = 3;
end
% 以下omegaVBB仅作为示例，实际仿真时根据需要进行改动
if cfg.enableCBVdynamic
    in.omegaVBB.time = in.omegaNVV.time;
    in.omegaVBB.signals.dimensions = 3;
    in.omegaVBB.signals.values = zeros(length(in.omegaNVV.time), 3);
    symbol = 1;
    dt = in.omegaVBB.time(2) - in.omegaVBB.time(1);
    for i=1:length(in.omegaVBB.time)
        if mod(in.omegaVBB.time(i), par.CBVPeriod) == 0 && in.omegaVBB.time(i) ~= 0
            [ ~, sectomegaVBBValues, ~, ~ ] = geneletgi_rotabtbdfixaxis(in.omegaVBB.time(i), symbol * [0 0 pi]', pi/10, 1, 1, dt); % 利用现有函数
            in.omegaVBB.signals.values(i:i+length(sectomegaVBBValues)-1,:) = sectomegaVBBValues;
            symbol = -symbol; % 正转、反转结合
        end
    end
    in.omegaVBB.signals.values = in.omegaVBB.signals.values(1:length(in.omegaVBB.time),:);
end
if ~isfield(in, 'nav_fB')
    in.nav_fB.time = 0;
    in.nav_fB.signals.values = zeros(1, 3);
    in.nav_fB.signals.dimensions = 3;
end
if ~isfield(in, 'nav_omegaIBB')
    in.nav_omegaIBB.time = 0;
    in.nav_omegaIBB.signals.values = zeros(1, 3);
    in.nav_omegaIBB.signals.dimensions = 3;
end
%% 参量
par.simOutputTimes = (0:par.samplePeriod:par.tSects(end));
% 调整分段参数尺寸
nSects = length(par.tSects) - 1;
[par.IMU.acc.dfBiasS] = resizesectpar(par.IMU.acc.dfBiasS, nSects, 2, 'par.IMU.acc.dfBiasS');
[par.IMU.acc.dKScalP] = resizesectpar(par.IMU.acc.dKScalP, nSects, 3, 'par.IMU.acc.dKScalP');
[par.IMU.acc.dKScalN] = resizesectpar(par.IMU.acc.dKScalN, nSects, 3, 'par.IMU.acc.dKScalN');
[par.IMU.acc.dPSS0] = resizesectpar(par.IMU.acc.dPSS0, nSects, 3, 'par.IMU.acc.dPSS0');
[par.IMU.acc.dfQuantS] = resizesectpar(par.IMU.acc.dfQuantS, nSects, 2, 'par.IMU.acc.dfQuantS');
[par.IMU.acc.lB] = resizesectpar(par.IMU.acc.lB, nSects, 3, 'par.IMU.acc.lB');
[par.IMU.acc.KAniso] = resizesectpar(par.IMU.acc.KAniso, nSects, 2, 'par.IMU.acc.KAniso');
[par.IMU.gyro.domegaIBBiasS] = resizesectpar(par.IMU.gyro.domegaIBBiasS, nSects, 2, 'par.IMU.gyro.domegaIBBiasS');
[par.IMU.gyro.dKScalP] = resizesectpar(par.IMU.gyro.dKScalP, nSects, 3, 'par.IMU.gyro.dKScalP');
[par.IMU.gyro.dKScalN] = resizesectpar(par.IMU.gyro.dKScalN, nSects, 3, 'par.IMU.gyro.dKScalN');
[par.IMU.gyro.dPSS0] = resizesectpar(par.IMU.gyro.dPSS0, nSects, 3, 'par.IMU.gyro.dPSS0');
[par.IMU.gyro.domegaIBQuantS] = resizesectpar(par.IMU.gyro.domegaIBQuantS, nSects, 2, 'par.IMU.gyro.domegaIBQuantS');
[par.IMU.gyro.DGSens] = resizesectpar(par.IMU.gyro.DGSens, nSects, 3, 'par.IMU.gyro.DGSens');
if ~all(par.IMU.acc.noise.Q)
    par.IMU.acc.noise.Q = ones(3, 1)*realmin;
end
if ~all(par.IMU.gyro.noise.Q)
    par.IMU.gyro.noise.Q = ones(3, 1)*realmin;
end
par.lV = [zeros(3, 1) par.displm.lV par.IMU.lV]; % 第一列表示V系原点，从第2列开始依次为各传感器杆臂参数（目前包括位移计、惯组）

%% 初始化
n = size(par.lV, 2);

iniCon.pos = repmat(iniCon.pos,1,n);
for i = 1:n
    iniCon.pos(:, i) = levarmcomp_pos(iniCon.pos(:, i), iniCon.CVG, -par.lV(:, i), cst.f, cst.RE); % TODO\LM: levarmcomp_pos移入本库
end

%% 运行仿真
sim('gentrajectorymdl', par.simOutputTimes, cfg.simOptions);
disp('完成轨迹模型运行');

%% 轨迹生成器输出量
out.t = par.simOutputTimes';
out.CVG = trajGen_CVN;
out.CVL = NaN(size(out.CVG));
for i=1:size(out.CVG, 3)
    out.CVL(:, :, i) = cst.CNL * out.CVG(:, :, i);
end
out.vehicleAtt = trajGen_vehicleAtt;
out.CBG = trajGen_CBN;
out.CBL = NaN(size(out.CBG));
for i=1:size(out.CBG, 3)
    out.CBL(:, :, i) = cst.CNL * out.CBG(:, :, i);
end
out.bodyAtt = trajGen_bodyAtt;
if size(par.lV, 2) > 1
    out.fV =  permute(trajGen_fV, [3 1 2]);
    out.vG = permute(trajGen_vN, [3 1 2]);
    out.pos = permute(vertcat(trajGen_L, trajGen_lambda, trajGen_h), [3 1 2]);
    out.RE = permute(trajGen_RE, [3 1 2]);
    out.s = squeeze(trajGen_s)';
else
    out.fV =  trajGen_fV;
    out.vG = trajGen_vN;
    out.pos = horzcat(trajGen_L, trajGen_lambda, trajGen_h);
    out.RE = trajGen_RE;
    out.s = trajGen_s;
end
out.omegaNVV = trajGen_omegaNVV;
out.omegaIVV = trajGen_omegaIVV;
out.omegaENN = trajGen_omegaENN;
out.vNRate = trajGen_vNRate;
%% IMU输出量
out.IMU.fB = permute(IMU_fB, [3 1 2]);
out.IMU.omegaIBB = IMU_omegaIBB;
out.IMU.fBErrFree = permute(IMU_fBErrFree, [3 1 2]);
out.IMU.omegaIBBErrFree = IMU_omegaIBBErrFree;

% 加速度计输出
out.IMU.specVelInc = diff(permute(IMU_fBInt,[3 1 2])); % t时刻对应的是t-tsamplePeriod到t时刻的比力积分（比速度增量）值
out.IMU.accOut = NaN(size(out.IMU.specVelInc));
for i=1 : size(out.IMU.specVelInc, 1)
    out.IMU.accOut(i, :) = (par.IMU.acc.KScal0 .* out.IMU.specVelInc(i, :)')';
end

% 陀螺输出
out.IMU.angleInc = diff(IMU_omegaIBBInt); % t时刻对应的是t-tsamplePeriod到t时刻的角速度积分（角度增量）值
out.IMU.gyroOut = NaN(size(out.IMU.angleInc));
for i=1 : size(out.IMU.angleInc, 1)
    out.IMU.gyroOut(i, :) = (par.IMU.gyro.KScal0 .* out.IMU.angleInc(i, :)')';
end

% 加速度计的无误差输出
out.IMU.specVelIncErrFree = diff(permute(IMU_fBIntErrFree,[3 1 2])); % t时刻对应的是t-tsamplePeriod到t时刻的比力积分（比速度增量）值
out.IMU.accOutErrFree = NaN(size(out.IMU.specVelIncErrFree));
for i=1 : size(out.IMU.specVelIncErrFree, 1)
    out.IMU.accOutErrFree(i, :) = (par.IMU.acc.KScal0 .* out.IMU.specVelIncErrFree(i, :)')';
end

% 陀螺的无误差输出
out.IMU.angleIncErrFree = diff(IMU_omegaIBBIntErrFree); % t时刻对应的是t-tsamplePeriod到t时刻的角速度积分（角度增量）值
out.IMU.gyroOutErrFree = NaN(size(out.IMU.angleIncErrFree));
for i=1 : size(out.IMU.angleIncErrFree, 1)
    out.IMU.gyroOutErrFree(i, :) = (par.IMU.gyro.KScal0 .* out.IMU.angleIncErrFree(i, :)')';
end

% 量化输出
if cfg.IMU.quantizeOut
    out.IMU.accOut = quantize(out.IMU.accOut);
    out.IMU.accOutErrFree = quantize(out.IMU.accOutErrFree);
    out.IMU.gyroOut = quantize(out.IMU.gyroOut);
    out.IMU.gyroOutErrFree = quantize(out.IMU.gyroOutErrFree);
end

% 写入文件
if ~isempty(cfg.IMU.outFilePath)
    writeimuoutfile_tradfmt(cfg.IMU.outFilePath, out.IMU.accOut, out.IMU.gyroOut, out.t(2:end, :), cfg.IMU.outPrecision);
end
disp('完成轨迹输出量生成');

%% 位移计输出量
out.displm.vV = squeeze(displm_vV)';
out.displm.vVErrFree = squeeze(displm_vVErrFree)';

% 增量输出
out.displm.displInc = diff(squeeze(displm_vVInt)'); % t时刻对应的是t-tsamplePeriod到t时刻的速度积分（位移增量）值
out.displm.displmOut = NaN(size(out.displm.displInc));
for i=1 : size(out.displm.displInc, 1)
    out.displm.displmOut(i, :) = (par.displm.SF0 .* out.displm.displInc(i, :)')';
end

% 无误差增量输出
out.displm.displIncErrFree = diff(squeeze(displm_vVIntErrFree)'); % t时刻对应的是t-tsamplePeriod到t时刻的速度积分（位移增量）值
out.displm.displmOutErrFree = NaN(size(out.displm.displIncErrFree));
for i=1 : size(out.displm.displIncErrFree, 1)
    out.displm.displmOutErrFree(i, :) = (par.displm.SF0 .* out.displm.displIncErrFree(i, :)')';
end

% 量化输出
if cfg.displm.quantizeOut
    out.displm.displmOut = quantize(out.displm.displmOut);
    out.displm.displmOutErrFree = quantize(out.displm.displmOutErrFree);
end

%% 辅助输出量
auxOut.nav.CBG = nav_CBN;
auxOut.nav.att = nav_att;
auxOut.nav.vG = nav_vN;
auxOut.nav.pos = horzcat(nav_L, nav_lambda, nav_h);
if isfield(in, 'genFuncName') && ~isempty(in.genFuncName)
    auxOut.TGI.vVRate = in.vVRate;
    auxOut.TGI.omegaNVV = in.omegaNVV;
    auxOut.TGI.omegaNVVRate = in.omegaNVVRate;
end
auxOut.TGI.tSects = par.tSects;
%% 保留本次仿真参数及结果
% NOTE: 一起保存以防止函数运行中断后输入及输出不一致
p_genTraj_in = originalIn;
p_genTraj_cfg = originalCfg;
p_genTraj_par = originalPar;
p_genTraj_iniCon = originalIniCon;
p_genTraj_cst = originalCst;
p_genTraj_out = out;
p_genTraj_auxOut = auxOut;
end

function [par] = resizesectpar(par, lastDimTargetSize, nParDim, parName)
if nParDim == 2
    lastDimSize = size(par, 2);
    if lastDimSize < lastDimTargetSize
        par = [par repmat(par(:, end), 1, lastDimTargetSize-lastDimSize)];
        if lastDimSize ~= 1
            warning('gentrajectory:wrongparsize', [parName '列数小于轨迹段数（' int2str(lastDimTargetSize) '），用最后一列补齐。']);
        end
    elseif lastDimSize > lastDimTargetSize
        par = par(:, 1:lastDimTargetSize);
        warning('gentrajectory:wrongparsize', [parName '列数大于轨迹段数（' int2str(lastDimTargetSize) '），截掉多余列。']);
    end
elseif nParDim == 3
    lastDimSize = size(par, 3);
    if lastDimSize < lastDimTargetSize
        par = cat(3, par, repmat(par(:, :, end), [1 1 lastDimTargetSize-lastDimSize]));
        if lastDimSize ~= 1
            warning('gentrajectory:wrongparsize', [parName '页数小于轨迹段数（' int2str(lastDimTargetSize) '），用最后一页补齐。']);
        end
    elseif lastDimSize > lastDimTargetSize
        par = par(:, :, 1:lastDimTargetSize);
        warning('gentrajectory:wrongparsize', [parName '页数大于轨迹段数（' int2str(lastDimTargetSize) '），截掉多余页。']);
    end
else
    assert(false);
end
end